/* 
 * File:   main.c
 * Title:  NES / SNES Replay Device
 * Author: true
 *
 * File created on September 8, 2013, 9:28 PM
 *
 * This project is licensed under the terms of the WTFPL, with an amendment.
 * See copying.txt for license details. The amendment is as follows:
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 *
 * You may, at your option, attribute the Author of this work, or the Title of
 * this work, in your project so long as such attribution adheres to the terms
 * of the WTFPL.
 *
 * ---
 *
 * MCU always runs at 42MHz on the internal oscillator.
 *   (NOTE: Current bootloader will switch to external oscillator.)
 * Should work with standard 40MHz rated MCUs - you do not need to buy a 50MHz rated MCU.
 * Code may be portable to (and work well on) 80MHz MCUs.
 *
 * The external 8MHz crystal is optional with the standard firmware
 * and required for USB firmware.
 *
 * It is (likely) safe to use a USB firmware on a device that either
 * does not support USB, or supports USB but is without the crystal.
 * Obviously, USB will not work in this case :)
 *
 * ---
 *
 * $Id: main.c 46 2014-01-03 03:53:26Z true $
 */

#define VER_MAJOR		1
#define VER_MINOR		0
#define VER_SUBMINOR	'f'

#define SYS_FREQ		42000000L
#define PB_FREQ		SYS_FREQ


/* configuration mostly auto-generated by mplabx */
#include <xc.h>

// DEVCFG3
// USERID = No Setting
#pragma config PMDL1WAY = OFF           // Peripheral Module Disable Configuration (Allow any reconfiguration)
#pragma config IOL1WAY = OFF            // Peripheral Pin Select Configuration (Allow any reconfiguration)
#pragma config FUSBIDIO = OFF           // USB USID Selection (Controlled by Port Function)
#pragma config FVBUSONIO = OFF          // USB VBUS ON Selection (Controlled by Port Function)

// DEVCFG2
#pragma config FPLLIDIV = DIV_2         // PLL Input Divider (2x Divider)
#pragma config FPLLMUL = MUL_21         // PLL Multiplier (21x Multiplier)
#pragma config UPLLIDIV = DIV_2         // USB PLL Input Divider (2x Divider)
#pragma config UPLLEN = ON              // USB PLL Enable (Enabled)
#pragma config FPLLODIV = DIV_2         // System PLL Output Clock Divider (PLL Divide by 2)

// DEVCFG1
#pragma config FNOSC = FRCPLL           // Oscillator Selection Bits (Fast RC Osc (FRC))
#pragma config FSOSCEN = OFF            // Secondary Oscillator Enable (Disabled)
#pragma config IESO = OFF               // Internal/External Switch Over (Disabled)
#pragma config POSCMOD = HS             // Primary Oscillator Configuration (HS osc mode)
#pragma config OSCIOFNC = OFF           // CLKO Output Signal Active on the OSCO Pin (Disabled)
#pragma config FPBDIV = DIV_1           // Peripheral Clock Divisor (Pb_Clk is Sys_Clk/1)
#pragma config FCKSM = CSECME           // Clock Switching and Monitor Selection (Clock Switch Enable, FSCM Enabled)
#pragma config WDTPS = PS1048576        // Watchdog Timer Postscaler (1:1048576)
#pragma config WINDIS = OFF             // Watchdog Timer Window Enable (Watchdog Timer is in Non-Window Mode)
#pragma config FWDTEN = OFF             // Watchdog Timer Enable (WDT Disabled (SWDTEN Bit Controls))
#pragma config FWDTWINSZ = WISZ_25      // Watchdog Timer Window Size (Window Size is 25%)

// DEVCFG0
#pragma config JTAGEN = OFF             // JTAG Enable (JTAG Port Enabled)
#pragma config ICESEL = ICS_PGx1        // ICE/ICD Comm Channel Select (Communicate on PGEC1/PGED1)
//#pragma config PWP = PWP12K             // Program Flash Write Protect (First 12K)
#pragma config PWP = OFF                // Program Flash Write Protect (Disabled)
#pragma config BWP = OFF                // Boot Flash Write Protect bit (Protection Disabled)
#pragma config CP = OFF                 // Code Protect (Protection Disabled)
//#pragma config DEBUG = OFF

// end config


#include <stdio.h>
#include <stdlib.h>

#include <plib.h>
#include <sys/attribs.h>

#ifdef USE_USB
#include "usb/usb.h"
#include "usb/usb_function_cdc.h"
#endif


#define	UNLAGGED			0
#define	LAGGED			1

#define	MODE_NES			0
#define	MODE_SNES_5		0
#define	MODE_SNES_7_2P		1
#define	MODE_SNES_7_1P2P	2
#define	MODE_SNES_7_1P		3

#define	START_NORMAL		0
#define	START_SKIPFIRST	1

#define	VSYNCD_OFF		0
#define	VSYNCD_STARTUP		1
#define	VSYNCD_STARTUP2	2
#define	VSYNCD_ON			3
#define	VSYNCD_WAITING		4

#define	HZ_PAL			(SYS_FREQ / 50) >> 5
#define	HZ_NTSC			(SYS_FREQ / 60) >> 5
#define	MS				(SYS_FREQ / 1000)

#define	DATA_BUF_SIZE		32				// must be power of 2


/* nes controller io assignments */
const IoPortId iopin_port	= IOPORT_B;
const int	iopin_latch		= BIT_13;			// all of these are on PORTB.
const int iopin_clock[2]		= {BIT_7, BIT_14};
const int iopin_data0[2]		= {BIT_8, BIT_2};
const int iopin_data1[2]		= {BIT_9, BIT_3};
const int iopin_ioaux[2]		= {BIT_15, BIT_5};

const IoPortId led_port = IOPORT_A;			// blinkenlights
const int led_pin = BIT_0;

const IoPortId btn_port = IOPORT_A;			// pushbutton and misc?
const int btn_pin = BIT_1;


/* nes controller data handling */
unsigned int		data[2][4][DATA_BUF_SIZE];
unsigned int		*data0_ptr[4];			// faster lookup of data0
unsigned int		*data1_ptr[4];			// faster lookup of data1

UINT8			data_write[4];
UINT8			data_read[4];			// need to separate buffer lookup for multitap readout

UINT8			data_mask[2];			// clock count for data (eg. nes = 8, snes = 16)

UINT8			latch[2] = {0, 0};		// amount of latches this frame
UINT8			latch_dir;
									// (tracked per port for data load purposes)
UINT8			latches_per_frame;		// expected latches per frame
UINT8			vsync_detect;			// external lm1881 circuit

UINT8			clock[4];				// currently clocked output

volatile UINT8		ioaux_state[2] = {0, 0};	// ioaux pin input state

/* control mode */
UINT8			mode = MODE_NES;		// 0 = nes, 1 = 5-wire snes, 2 = 7-wire snes
UINT8			skiplatch = START_NORMAL;// ghost latch on poweron, not reset.


/* frame and lag counters */
unsigned int		framecount[2];			// 0 = std, 1 = lag

unsigned int		frameperiod;			// timer4 frame length (set with ~c command)
										// set to fffe=learn, ffff=learning, <ff00=learned
UINT8			lagdet_ena;			// enable lag detect (increments buffer on lag)
unsigned int		lastlag;				// consecutive lagged frame count

unsigned int		lag_reset_frames = 60;	// default consecutive lag frames considered a reset

unsigned int		console_is_reset;		// movie isn't playing


/* uart buffers */
int				uart_baud = 115200;

unsigned char		uart_tx_buf[32];

volatile UINT8		uart_rx_len = 0xff;
unsigned char		uart_rx_buf[72];
volatile int		uart_rx_timeout;

volatile int		uart_err_count; // debug


/* timer1 mainline isr stuff*/
unsigned int		led_flash;
unsigned int		led_count;

unsigned int		btn_count[2];


/* usb buffers */
#ifdef USE_USB
#define USB_READY	!(USBDeviceState < CONFIGURED_STATE)||(USBSuspendControl==1)

unsigned char		usb_tx_buf[CDC_DATA_OUT_EP_SIZE];
unsigned char		next_usb_tx;

unsigned char		usb_rx_buf[CDC_DATA_IN_EP_SIZE];
unsigned char		usb_rx_len;
#endif


/*
 * uart config
 */
void setup_uart()
{
	UARTConfigure(UART1, UART_ENABLE_PINS_TX_RX_ONLY);
	UARTSetFifoMode(UART1, UART_INTERRUPT_ON_TX_NOT_FULL | UART_INTERRUPT_ON_RX_NOT_EMPTY);
	UARTSetLineControl(UART1, UART_DATA_SIZE_8_BITS | UART_PARITY_NONE | UART_STOP_BITS_1);
	uart_baud = UARTSetDataRate(UART1, PB_FREQ, uart_baud);
	UARTEnable(UART1, UART_ENABLE | UART_PERIPHERAL | UART_RX | UART_TX);

	INTSetVectorPriority(INT_UART_1_VECTOR, INT_PRIORITY_LEVEL_1);
	INTSetVectorSubPriority(INT_UART_1_VECTOR, INT_SUB_PRIORITY_LEVEL_0);
	
	// interrupt on receive and error only
	IEC1bits.U1TXIE = 0;
	IEC1bits.U1RXIE = 1;
	IEC1bits.U1EIE = 1;
}

/*
 * send data to host
 */
void SerSendDataByte(BYTE byte)
{
	while(!UARTTransmitterIsReady(UART1));
	UARTSendDataByte(UART1, byte);

	#ifdef USE_USB
	if (next_usb_tx < CDC_DATA_OUT_EP_SIZE) {
		usb_tx_buf[next_usb_tx] = byte;
		next_usb_tx++;
	}
	#endif
}

/*
 * parse data received from host
 */
void parse_uart()
{
	int done;
	int player;
	int i;

	done = 0;
	player = 0;

	switch (uart_rx_buf[0]) {
		case '1':
		case '2':
		case '3':
		case '4': {	// data buttons
			if (uart_rx_len >= 9) {
				player = uart_rx_buf[0] - '1';

				if (player < 4) {
					data[0][player][data_write[player]] = (uart_rx_buf[1] << 24 | uart_rx_buf[2] << 16 |
							uart_rx_buf[3] << 8 | uart_rx_buf[4]);
					data[1][player][data_write[player]] = (uart_rx_buf[5] << 24 | uart_rx_buf[6] << 16 |
							uart_rx_buf[7] << 8 | uart_rx_buf[8]);

					data_write[player]++;
					data_write[player] &= (DATA_BUF_SIZE - 1);
				}
				done = 1;
			}
			break;
		}
		case 'c': {	// force to 50fps or 60fps mode
			if (uart_rx_len >= 2) {
				if (uart_rx_buf[1] == '5') {
					frameperiod = HZ_PAL;
				}
				if (uart_rx_buf[1] == '6') {
					frameperiod = HZ_NTSC;
				}
				done = 1;
			}
			break;
		}
		case 'g': {
			if (uart_rx_len >= 2) {
				lagdet_ena = uart_rx_buf[1] == '1' ? 1 : 0;
				done = 1;
			}
			break;
		}
		case 'l': {
			if (uart_rx_len >= 2) {
				if (uart_rx_buf[1] <= '9') {
					latches_per_frame = uart_rx_buf[1] - '0';
				} else if (uart_rx_buf[1] == 'V') {
					latches_per_frame = 255;
				} else if (uart_rx_buf[1] <= 'z') {
					latches_per_frame = 11 + (uart_rx_buf[1] - 'a');
				} 
				done = 1;
			}
			break;
		}
		case 'r': {	// reset playback buffer, this happens on the next latch
			for (i = 0; i <= 3; i++) {
				data_write[i] = 0;
				data_read[i] = 0;
				data0_ptr[i] = &data[0][i][0];
				data1_ptr[i] = &data[1][i][0];

				for (done = 0; done < DATA_BUF_SIZE; done++) {
					data[0][i][done] = 0;
					data[1][i][done] = 0;
				}
			}

			vsync_detect = VSYNCD_OFF;
			break;
		}
		case 's': {	// nes skip first ghost latch
			/* when the NES starts, it will usually set the clock line high,
			 * followed very shortly (~5usec) by latch going high.
			 * This latch stays high for >20ms before going low.
			 * Usually you will want to send the "~s1" command when replaying NES.
			 * This doesn't happen on SNES.
			 */
			if (uart_rx_len >= 2) {
				if (uart_rx_buf[1] == '0') {
					skiplatch = START_NORMAL;
				} else {
					if (uart_rx_buf[1] > '0' && uart_rx_buf[1] <= '9') {
						skiplatch = uart_rx_buf[1] - '0';
					} else {
						skiplatch = START_SKIPFIRST;
					}
				}
				done = 1;
			}
			break;
		}
		case 't': {	// timeout value
			if (uart_rx_len >= 3) {
				lag_reset_frames = (uart_rx_buf[1] << 8 | uart_rx_buf[2]);
				done = 1;
			}
			break;
		}
		case 'v': {	// respond immediately with firmware version
			SerSendDataByte('V');
			SerSendDataByte(VER_MAJOR);
			SerSendDataByte(VER_MINOR);
			SerSendDataByte(VER_SUBMINOR);
			done = 1;
			break;
		}
		case 'M': {	// bitmask setup (8, 16, 32bit) (ex: M18)
			if (uart_rx_len >= 3) {
				if (uart_rx_buf[1] == '1' || uart_rx_buf[1] == '2') {
					player = uart_rx_buf[1] - '1';
					if (player == 0 || player == 1) {
						if (uart_rx_buf[2] == '8') data_mask[player] = 8;
						if (uart_rx_buf[2] == '6') data_mask[player] = 16;
						if (uart_rx_buf[2] == '2') data_mask[player] = 32;
					}
				}
				done = 1;
			}
			break;
		}
	}

	if (done) {
		uart_rx_len = 0xff;
	}
}

/*
 * get data received from host
 */
void SerGetDataByte(BYTE byte)
{
	int i;
	
	if (uart_rx_len < 70) {					// receiving valid data
		uart_rx_buf[uart_rx_len] = byte;
		uart_rx_len++;
		uart_rx_timeout = 0;

		parse_uart();
	} else if (byte == '~') {				// not receiving, find start byte
		for (i = 0; i < 70; i++) {
			uart_rx_buf[i] = '\0';
		}
		uart_rx_len = 0;
		uart_rx_timeout = 0;
	} else {								// not receiving, invalid byte
		uart_rx_len = 0xff;
	}
}


/* frame counter and lagframe detection */
inline void frametimer_init(unsigned int period)
{
	CloseTimer4();
	WriteTimer4(0);
	OpenTimer4(T4_ON | T4_SOURCE_INT | T4_PS_1_32, period);
	ConfigIntTimer4(T4_INT_ON | T4_INT_PRIOR_3);
}

inline void datatimer_init(unsigned int period)
{
	CloseTimer5();
	WriteTimer5(0);
	OpenTimer5(T5_ON | T5_SOURCE_INT | T5_PS_1_32, period);
	ConfigIntTimer5(T5_INT_ON | T5_INT_PRIOR_2 | T5_INT_SUB_PRIOR_3);
}

inline void lagtimer_init(unsigned int period)
{
	CloseTimer2();
	WriteTimer2(0);
	OpenTimer2(T2_ON | T2_SOURCE_INT | T2_PS_1_32, period);
	ConfigIntTimer2(T2_INT_ON | T2_INT_PRIOR_2 | T2_INT_SUB_PRIOR_2);
}

/*
 * set buffers to point at player's next frame of data
 */
void nesPrepareNext(int player)
{
	// since we're loading a new byte, and only do that after clocking out,
	// we reset our latch count
	latch[player] = 0;

	// we need the multitap data readout to track where the standard readout was
	// btw: this is a hack
	if (player < 2) {
		data_read[player + 2] = data_read[player];
	}

	// increment buffer
	data_read[player]++;
	data_read[player] &= (DATA_BUF_SIZE - 1);

	// set pointers to data
	// we use these pointers as lookups are slow in the latch and clock handlers
	//data0_ptr[player] = &data[0][player][data_read[player]];
	//data1_ptr[player] = &data[1][player][data_read[player]];
	// this version uses more space but executes less code, faster response
	if (data_read[player]) {
		data0_ptr[player]++;
		data1_ptr[player]++;
	} else {
		data0_ptr[player] = &data[0][player][0];
		data1_ptr[player] = &data[1][player][0];
	}

	if (player == 0) {
		// cancel Timer2 lag detect
		CloseTimer2();
		// and restart frametimer
		if (latches_per_frame < 255) {
			frametimer_init(frameperiod);
		}

		// we had a valid frame; tell host we need another
		SerSendDataByte('F');
	}

	// keep track of total frames (why? debugging?)
	framecount[UNLAGGED]++;
}


/*
 * controller latch handler
 */
static inline void nesLatch(void)
{
	UINT8 p;

	// set initial data (p1)
	p = (ioaux_state[0]) ? 2 : 0;
	if (*data0_ptr[p] & (1 << 31)) {
		PORTClearBits(iopin_port, iopin_data0[0]);
	} else {
		PORTSetBits(iopin_port, iopin_data0[0]);
	}
	if (*data1_ptr[p] & (1 << 31)) {
		PORTClearBits(iopin_port, iopin_data1[0]);
	} else {
		PORTSetBits(iopin_port, iopin_data1[0]);
	}

	// set initial data (p2)
	p = (ioaux_state[1]) ? 3 : 1;
	if (*data0_ptr[p] & (1 << 31)) {
		PORTClearBits(iopin_port, iopin_data0[1]);
	} else {
		PORTSetBits(iopin_port, iopin_data0[1]);
	}
	if (*data1_ptr[p] & (1 << 31)) {
		PORTClearBits(iopin_port, iopin_data1[1]);
	} else {
		PORTSetBits(iopin_port, iopin_data1[1]);
	}

	// we do this after the latch so we don't slow it down.
	// if we are ignoring input because of the startup latch glitch, then quit now.
	if (skiplatch) {
		skiplatch--;
		return;
	}

	console_is_reset = 0;
	CloseTimer2();

	// if we are in frame latch window mode, enable the timeout
	if (latches_per_frame > 8 && latches_per_frame < 255) {
		datatimer_init((MS >> 6) * (latches_per_frame - 10));
	}

	// increase per-frame latch count
	latch[0]++;
	latch[1]++;
	
	// latch resets clock count
	clock[0] = clock[1] = clock[2] = clock[3] = 0;
}

/*
 * controller clock handler
 */
void nesClock(int player)
{
	UINT8 p;

	p = (ioaux_state[player]) ? player + 2 : player;
	clock[p]++;

	// have we clocked out all of our data? (assumed len(data1) == len(data0))
	if (clock[p] >= data_mask[player]) {
		// (s)nes has data low when not active
		PORTClearBits(iopin_port, iopin_data0[player]);
		PORTClearBits(iopin_port, iopin_data1[player]);

		// clear lag handler - this was a valid frame
		if (p == 0) {
			lastlag = 0;
			PORTClearBits(led_port, led_pin);		// and let's make sure the lag LED is off
		}

		// prepare next latch for next data in fixed latches-per-frame mode
		// DO NOT use this mode with multitap games; it will not work (use window mode)
		if (latches_per_frame <= 8 && latch[player] >= latches_per_frame) {
			nesPrepareNext(p);
		}

		// latch window mode is handled by nesLatch and main timer

		// if in vsync detect mode, turn it on now
		if (latches_per_frame == 255) {
			vsync_detect = VSYNCD_STARTUP;
		}
		
		return;
	}

	// clock out
	if ((*data0_ptr[p] << clock[p]) & (1 << 31)) {
		PORTClearBits(iopin_port, iopin_data0[player]);
	} else {
		PORTSetBits(iopin_port, iopin_data0[player]);
	}

	if ((*data1_ptr[p] << clock[p]) & (1 << 31)) {
		PORTClearBits(iopin_port, iopin_data1[player]);
	} else {
		PORTSetBits(iopin_port, iopin_data1[player]);
	}
}

/*
 * controller ioaux input handler
 */
void snesIOChange(int player, UINT8 flag)
{
	int p;

	// only update if different
	if (ioaux_state[player] != flag) {
		ioaux_state[player] = flag;
		p = flag ? player + 2 : player;
		
		if (clock[p] >= data_mask[player]) {
			// this port has already been clocked out (this happens usually when going from lo to hi)
			PORTClearBits(iopin_port, iopin_data0[player]);
			PORTClearBits(iopin_port, iopin_data1[player]);
		} else {
			// set bit to currently clocked value (usually set to 0 from hi to lo)
			if ((*data0_ptr[p] << clock[p]) & (1 << 31)) {
				PORTClearBits(iopin_port, iopin_data0[player]);
			} else {
				PORTSetBits(iopin_port, iopin_data0[player]);
			}
			if ((*data1_ptr[p] << clock[p]) & (1 << 31)) {
				PORTClearBits(iopin_port, iopin_data1[player]);
			} else {
				PORTSetBits(iopin_port, iopin_data1[player]);
			}
		}
	}
}


int main(int argc, char** argv) {
	int i;

	while(!OSCCONbits.SLOCK); //Wait for the PLL to lock
	
	// PIC setup
	SYSTEMConfig(SYS_FREQ, SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE);

	// disable analog
	ANSELA = 0;
	ANSELB = 0;

	// PPS setup
	SYSKEY = 0x0;
     SYSKEY = 0xAA996655;
     SYSKEY = 0x556699AA;
     CFGCONbits.IOLOCK = 0;

	PPSInput(3, INT2, RPB13);	// latch
	/* INT0 is non-assignable */	// p1 clock is INT0 pin 16 PB7
	/* this can be polled */		// p1 iobit-input
	PPSInput(4, INT1, RPB14);	// p2 clock
	/* this can be polled */		// p2 iobit-input

	PPSInput(3, U1RX, RPA4);		// uart rx
	PPSOutput(1, RPB4, U1TX);	// uart tx

	CFGCONbits.IOLOCK = 1;

	// Pin setup
	PORTClearBits(iopin_port, iopin_data0[0]);
	PORTClearBits(iopin_port, iopin_data1[0]);
	PORTClearBits(iopin_port, iopin_data0[1]);
	PORTClearBits(iopin_port, iopin_data1[1]);

	PORTSetPinsDigitalIn(iopin_port, iopin_latch);

	PORTSetPinsDigitalIn(iopin_port, iopin_clock[0]);
	PORTSetPinsDigitalIn(iopin_port, iopin_ioaux[0]);
	PORTSetPinsDigitalOut(iopin_port, iopin_data0[0]);
	PORTSetPinsDigitalOut(iopin_port, iopin_data1[0]);

	PORTSetPinsDigitalIn(iopin_port, iopin_clock[1]);
	PORTSetPinsDigitalIn(iopin_port, iopin_ioaux[1]);
	PORTSetPinsDigitalOut(iopin_port, iopin_data0[1]);
	PORTSetPinsDigitalOut(iopin_port, iopin_data1[1]);

	PORTClearBits(led_port, led_pin);
	PORTSetPinsDigitalOut(led_port, led_pin);

	PORTSetPinsDigitalIn(btn_port, btn_pin);

	PORTClearBits(iopin_port, BIT_4); // uart tx

	//ODCBbits.ODCB8 = 1;   // data outputs open drain on 5V compatible outs
	//ODCBbits.ODCB9 = 1;

	/* note: latch and clock are active high, but nes/snes latch is usually low, clock usually high.
	 * if we update on the falling edge of clock, we might update too fast for the SNES to handle.
	 * so we try to make clocking as fast as possible while respecting clocking out on rising edge.
	 */

	// set up external interrupts
	ConfigINT2(EXT_INT_PRI_4 | EXT_INT_SUB_PRI_0 | RISING_EDGE_INT | EXT_INT_ENABLE);  // latch
	ConfigINT1(EXT_INT_PRI_5 | EXT_INT_SUB_PRI_0 | RISING_EDGE_INT | EXT_INT_ENABLE);  // p2 clock
	ConfigINT0(EXT_INT_PRI_6 | EXT_INT_SUB_PRI_0 | RISING_EDGE_INT | EXT_INT_ENABLE);  // p1 clock

	// set up iobit change notification interrupts
	mCNBOpen(CNB_ON | CNB_IDLE_CON, CNB15_ENABLE | CNB5_ENABLE, CNB_PULLUP_DISABLE_ALL);
	ConfigIntCNB(CHANGE_INT_PRI_3 | CHANGE_INT_ON);

	// variable setup
	framecount[UNLAGGED] = 0;
	framecount[LAGGED] = 0;

	lastlag = 0;
	console_is_reset = 1;

	clock[0] = 0;
	clock[1] = 0;

	latch_dir = 1;		// default nes mode

	// frame lengths
	/*
	frame_low[0] = (PB_FREQ / 60 / 32) - (PB_FREQ >> 17);
	frame_high[0] = (PB_FREQ / 60 / 32) + (PB_FREQ >> 17);

	frame_low[1] = (PB_FREQ / 50 / 32) - (PB_FREQ >> 17);
	frame_high[1] = (PB_FREQ / 50 / 32) + (PB_FREQ >> 17);
	*/

	// default to 60fps
	frameperiod = HZ_NTSC;

	// turn off vsync detect
	vsync_detect = VSYNCD_OFF;

	// default controller bitlength setup
	data_mask[0] = 8;
	data_mask[1] = 8;

	// initialize buffers
	for (i = 0; i <= 3; i++) {
		data_write[i] = 0;
		data_read[i] = 0;

		data[0][i][0] = 0;
		data[1][i][0] = 0;

		data0_ptr[i] = &data[0][i][0];
		data1_ptr[i] = &data[1][i][0];
	}

	// default latches per frame
	latches_per_frame = 1;

	// lag detect off by default
	lagdet_ena = 0;

	// set up uart
	setup_uart();

	// set up main timer (button handler, led flash, etc)
	OpenTimer1(T1_ON | T1_SOURCE_INT | T1_PS_1_8, SYS_FREQ / 8000);
	ConfigIntTimer1(T1_INT_ON | T1_INT_PRIOR_1 | T1_INT_SUB_PRIOR_3);
	
	// and finally, here we go
	INTEnableSystemMultiVectoredInt();
	
	#ifdef USE_USB
	InitializeUSB();

	// clear usb buffer
	usb_rx_len = 0;
	#endif

	SerSendDataByte('B');			// rebooted

	while (1) {
		#ifdef USE_USB
		USBDeviceTasks();

		if (USB_READY) {
			// get data from usb, shove it into uart buffer and process
			usb_rx_len = getsUSBUSART(usb_rx_buf, CDC_DATA_IN_EP_SIZE);
			if (usb_rx_len) {
				for (i = 0; i < usb_rx_len; i++) {
					SerGetDataByte(usb_rx_buf[i]);
				}
			}			

			// send data to usb
			if (uart_rx_len == 0xff) {
				if((USBUSARTIsTxTrfReady()) && (next_usb_tx > 0)) {
					putUSBUSART(&usb_tx_buf[0], next_usb_tx);
					next_usb_tx = 0;
				}
			}

			CDCTxService();
		}
		#endif

		// if in vsync detect mode, advance the buffer
		if (latches_per_frame == 255) {
			if (ioaux_state[0] && vsync_detect == VSYNCD_ON) {
				vsync_detect = VSYNCD_WAITING;
				nesPrepareNext(0);
				nesPrepareNext(1);
				if (mode > MODE_NES) {
					nesPrepareNext(2);
					nesPrepareNext(3);
				}
			}
			if (!ioaux_state[0] && vsync_detect == VSYNCD_WAITING) {
				vsync_detect = VSYNCD_ON;
			}
		}

		// reset device if too many simultaneous lagframes detected
		if (lastlag > lag_reset_frames) {
			lastlag = 0;

			console_is_reset = 1;
			CloseTimer2();

			framecount[UNLAGGED] = 0;
			framecount[LAGGED] = 0;
		}
	}
}

/* isr */
void __ISR(_UART_1_VECTOR, IPL1) uart1Handler(void) {
	if (IFS1bits.U1RXIF) {
		char rxbuf;

		rxbuf = UARTGetDataByte(UART1);

		SerGetDataByte(rxbuf);

		IFS1bits.U1RXIF = 0;
	}

	if (IFS1bits.U1EIF) {
		IFS1bits.U1EIF = 0;
		uart_err_count++;
	}
}

void __ISR(_TIMER_1_VECTOR, IPL1) timer1Handler(void) {
	int i;
	
	mT1ClearIntFlag();

	// vsync detect
	if (vsync_detect == VSYNCD_STARTUP2) {
		// we've waited at least 1ms, so we can turn this on now.
		vsync_detect = VSYNCD_ON;
	}
	if (vsync_detect == VSYNCD_STARTUP) {
		// we need to wait at least 1ms because of lm1881 trigger delay.
		// if we turn it on now, it might advance the buffer too early.
		// so wait a bit longer.
		vsync_detect = VSYNCD_STARTUP2;
	}

	// uart timeout detect
	if (uart_rx_len < 70) {
		uart_rx_timeout++;
		
		if (uart_rx_timeout == 2000) {
			for (i = 0; i < 70; i++) {
				uart_rx_buf[i] = '\0';
			}
			uart_rx_len = 0xff;
			uart_rx_timeout = 0;
		}
	}

	// led idle blinker
	if (console_is_reset) {
		led_count++;
		
		if (led_flash) {
			switch (mode) {
				case 0: {
					break;
				}
				case 1: {
					if (led_count == 120 || led_count == 270) {
						PORTToggleBits(led_port, led_pin);
					}
					break;
				}
				case 2: {
					if (led_count == 80 || led_count == 160
							|| led_count == 240 || led_count == 320) {

						PORTToggleBits(led_port, led_pin);
					}
					break;
				}
				case 3: {
					if (led_count == 40 || led_count == 360) {
						PORTToggleBits(led_port, led_pin);
					}
					break;
				}
			}
		}

		if (led_count > 400) {
			led_flash = !led_flash;
			led_count = 0;

			if (led_flash) {
				PORTSetBits(led_port, led_pin);
			} else {
				PORTClearBits(led_port, led_pin);
			}

			// re-inform host we're in reset status
			SerSendDataByte('R');
		}
	} else {
		led_count = 0;
	}

	// button handler (reset and change modes)
	if (!PORTReadBits(btn_port, btn_pin)) {
		btn_count[1] = 100;
		
		if (btn_count[0] == 20) {
			// set the lag reset timer back to default
			lag_reset_frames = 60;

			// and force a lastlag event
			lastlag = lag_reset_frames + 1;
		}
		if (btn_count[0] == 1000) {
			// set nes or snes mode
			mode++;
			if (mode > 3) {
				mode = 0;
			}

			if (mode <= MODE_NES) {
				ioaux_state[0] = 0;
				ioaux_state[1] = 0;

				latch_dir = 1;
			} else {
				latch_dir = 0;
			}
		}

		if (btn_count[0] < 65534) {
			btn_count[0]++;
		}
	} else {
		if (btn_count[1]) {
			btn_count[1]--;
			if (btn_count[1] == 0) {
				btn_count[0] = 0;
			}
		}
	}
}

void __ISR(_TIMER_2_VECTOR, IPL2) timer2Handler(void) {
	UINT8 i;

	mT2ClearIntFlag();
	CloseTimer2();

	if (!console_is_reset) {
		framecount[LAGGED]++;				// this frame is lagged
		lastlag++;

		PORTSetBits(led_port, led_pin);		// light our lag detected LED

		if (lagdet_ena) {
			for (i = 0; i <= 3; i++ ){
				data_read[i]++;
				data_read[i] &= (DATA_BUF_SIZE - 1);
			}
		}

		SerSendDataByte('L');				// send notice about this lag frame
	}
}

void __ISR(_TIMER_5_VECTOR, IPL2) timer5Handler(void) {
	mT5ClearIntFlag();
	CloseTimer5();

	nesPrepareNext(0);
	nesPrepareNext(1);
	if (mode > MODE_NES) {
		nesPrepareNext(2);
		nesPrepareNext(3);
	}
}

void __ISR(_TIMER_4_VECTOR, IPL3) timer4Handler(void) {
	int timer;

	mT4ClearIntFlag();

	// wait 15/16 of a frame to verify
	timer = frameperiod >> 1 | frameperiod >> 2 | frameperiod >> 3 | frameperiod >> 4;

	if (latches_per_frame > 8) {
		timer = timer - ((MS >> 6) * (latches_per_frame - 10));
	}

	lagtimer_init(timer);
}

void __ISR(_CHANGE_NOTICE_VECTOR, IPL3) CNHandler(void) {
	mPORTBRead();
	mCNBClearIntFlag();

	if (mode > MODE_NES) {
		snesIOChange(0, PORTReadBits(iopin_port, iopin_ioaux[0]) ? 0 : 1);
		snesIOChange(1, PORTReadBits(iopin_port, iopin_ioaux[1]) ? 0 : 1);
	}
}

void __ISR(_EXTERNAL_2_VECTOR, IPL4) nesLatchHandler(void) {
	if (latch_dir) {
		// bit is updated on falling edge for snes multitap,
		nesLatch();
		if (mode == MODE_NES) {
			// but on rising edge of nes because it is too fast.
			// rising works fine for snes standard mode too.
			mINT2ClearIntFlag();
			return;
		}
	} else {
		// for multitap, on rising edge, data0 is high, data1 is low.
		if (mode == MODE_SNES_7_1P || mode == MODE_SNES_7_1P2P) {
			PORTSetBits(iopin_port, iopin_data0[0]);
			PORTClearBits(iopin_port, iopin_data1[0]);
		}
		if (mode == MODE_SNES_7_2P || mode == MODE_SNES_7_1P2P) {
			PORTSetBits(iopin_port, iopin_data0[1]);
			PORTClearBits(iopin_port, iopin_data1[1]);
		}
	}

	// reset the edge detection
	mINT2IntEnable(0);
	mINT2SetEdgeMode(latch_dir);		// 1 = rising, 0 = falling
	mINT2ClearIntFlag();
	mINT2IntEnable(1);

	// change the state
	latch_dir = !latch_dir & 0x01;
}

void __ISR(_EXTERNAL_1_VECTOR, IPL5) nesClockHandler1(void) {
	nesClock(1);
	mINT1ClearIntFlag();
}

void __ISR(_EXTERNAL_0_VECTOR, IPL6) nesClockHandler0(void) {
	nesClock(0);
	mINT0ClearIntFlag();
}